;;;; -*- Mode: Lisp; Package: :dreaming-tree -*-
;;;;
;;;; Spatial sorting using r-trees.
;;;;

(In-package :dreaming-tree)

(defun r-tree-size (r)
  (with-slots (children) r
    (+ 1
       (case (length children)
	 (1 (cond ((typep (car children) 'node)
		   (r-tree-size (car children)))
		  (t 1)))
	 (t (multiple-value-bind (nodes leaves) (partition-if #'(lambda (x)
								  (typep x 'node))
							      children)
	      (+ (length leaves)
		 (iter (for n in nodes)
		       (summing (r-tree-size n))))))))))
			       

(defun find-minimum-bound (object overlapping-nodes)
  "Give a spatial, <object>, that intersects any number of other
spatials or nodes in <overlapping-nodes>, return the node (or spatial)
that grows the least to contain <object>."
  (let ((v0 (volume (bounding-volume object)))
	(c0 (center (bounding-volume object))))
    (iter (for node in overlapping-nodes)
	  (finding node minimizing (let ((v1 (volume (bounding-volume node)))
					 (c1 (center (bounding-volume node))))
				     (* (magnitude (m- c1 c0)) (+ v0 v1)))))))
	  
;; This is the slow way, but maybe is a bit clearer in what I am trying to do.
#|
  (iter (for node in overlapping-nodes)
	(finding node minimizing (volume (grow-to-contain (grow-to-contain (make-instance 'bounding-box)
									   (bounding-volume object))
							  (bounding-volume node))))))
|#

(defparameter *defer-interval-updates* t)

(defmethod insert-object ((o0 spatial) (o1 spatial))
  (let* ((parent (parent o1))
	 (node (make-instance 'r-tree :parent parent :bounding-volume (make-instance 'bounding-box))))
    (setf (children node) (list o0 o1)
	  (parent o0) node
	  (parent o1) node)
    (grow-to-contain node (children node))
    (unless *defer-interval-updates*
      (determine-intervals node t))
    (when parent
      (setf (children parent) (remove o1 (children parent)))
      (push node (children parent)))))

(defun nodep (obj)
  (eql (class-of obj) (find-class 'r-tree)))

(defmethod insert-object ((obj spatial) (root r-tree))
  (with-slots ((r_c children)) root
    ;(format t "Inserting ~A into ~A that has children ~A.~%" obj root r_c)
    (let ((overlaps (find-all obj r_c :test #'test-intersection)))
					; (format t "* Inserted object ~A overlaps ~A.~%" obj overlaps)
      (case (length overlaps)
	(0 (cond ((< (length r_c) (max-children root))
		  (progn
		    (push obj r_c)
		    (grow-to-contain root obj)
		    (unless *defer-interval-updates*
		      (determine-intervals root t))
		    (setf (parent obj) root)))
		 (t (insert-object obj (find-minimum-bound obj r_c))))) ; Causes infinite loop
	(1 (insert-object obj (car r_c)))
	(t (insert-object obj (find-minimum-bound obj overlaps)))))))

;; Some concerns:  
;;
;;  1) Should I recalculate the OBB of the r-tree node when deleting
;;     an element?
;;

(defmethod delete-object ((n r-tree) (parent r-tree))
  (setf (children parent) (delete n (children parent)))
  (dolist (c (children n))
    (insert-object c parent))
  (setf (children n) nil))

(defmethod delete-object ((obj spatial) (root r-tree))
  (with-slots (r_c) root
    (setf r_c (delete obj r_c))
    (if (< (length r_c) (min-children root))
	(delete-object root (parent root)))))

(defmethod visible-p ((cull basic-culler) (n r-tree))
  (visible-p cull (bounding-volume n)))

(defun resolve-tree-overlap (intersection)
  (let* ((a (car intersection)) (b (cdr intersection))
	 (node (if (nodep a)
		   a
		   b))
	 (obj (if (nodep a)
		  b
		  a))) ;; Nearly inane bookeeping...
    (perform-sweep (intervals node) (list (intervals obj)))))


;; Not right, appending does not ensure intervals are sorted!
(defun perform-sweep (sorted-intervals &optional (inserted nil))
  (destructuring-bind (x y z) sorted-intervals
    (when inserted
      (dolist (i inserted)
	(setf x (append x (first i))
	      y (append y (second i))
	      z (append z (third i)))))
    (if inserted
	(sweep-intervals-for-intersection (sort x #'e<) (sort y #'e<) (sort z #'e<))
	(sweep-intervals-for-intersection x y z))))

(defmethod find-intersecting-set ((root r-tree))
  ""
  (iter (for p in (perform-sweep (intervals root)))
	(if (or (nodep (car p)) 
		(nodep (cdr p)))
	    (collect (resolve-tree-overlap p))
	    (collect p))))


(defmethod determine-sorted-intervals ((r r-tree) &optional (local-only t))
  (let ((xi) (yi) (zi))
    (with-slots (intervals children) r
      (dolist (child children)
	(if (and (nodep child)
		 (not local-only))
	    (determine-sorted-intervals child nil))
	(destructuring-bind (x y z) (intervals-for child)
	  (setf xi (append xi x) 
		yi (append yi y)
		zi (append zi z))))
      (setf (child-x-intervals r) (sort xi #'e<)
	    (child-y-intervals r) (sort yi #'e<)
	    (child-z-intervals r) (sort zi #'e<)))))

;;;; -------- ;;;;
(defun test-insert ()
  (let ((obj0 (make-instance 'spatial))
	(obj1 (make-instance 'spatial))
	(r (make-instance 'r-tree))
	(c0 (make-cube 0.0 0.0 0.0))
	(c1 (make-cube 0.5 0.0 0.0))
	(c2 (make-cube 0.0 5.0 0.0)))
    (insert-object obj0 obj1)
    (insert-object c0 r)
    (insert-object c1 r)
    (insert-object c2 r)
    r))

(defun test-grow ()
  (let ((c0 (make-cube 0.0 0.0 0.0))
	(c1 (make-cube 1.0 0.0 0.0))
	(n (make-instance 'r-tree)))
    (grow-to-contain n c0)
    n))

(defun test-tree-intervals ()
  (let ((n (test-insert)))
    (determine-sorted-intervals n nil)
    n))


(defun ordered-set-of-cubes (n step)
  (let ((boxes))
    (dotimes (i n boxes)
      (push (make-cube (* i step) 0.0 0.0
		       :color (vector (random 1.0)
				      (random 1.0)
				      (random 1.0)
				      0.2))
	    boxes))))

(defun test-insert-2 ()
  (let ((boxes (ordered-set-of-cubes 100 5.0))
	(r (make-instance 'r-tree :bounding-volume (make-instance 'bounding-box))))
    (dolist (b boxes)
      (insert-object b r))
    r))

(defun test-insert-3 ()
  (let ((b1 (ordered-set-of-cubes 100 0.8))
	(b2 (ordered-set-of-cubes 100 1.1))
	(b3 (ordered-set-of-cubes 1000 1.5))
	(c0 (make-cube 0.0 0.0 0.0))
	(c1 (make-cube 0.0 1000.0 0.0))
	(r1 (make-instance 'r-tree :bounding-volume (make-instance 'bounding-box)))
	(r2 (make-instance 'r-tree :bounding-volume (make-instance 'bounding-box)))
	(r3 (make-instance 'r-tree :bounding-volume (make-instance 'bounding-box))))
  (format t "Time for heavily overlapped inserts.~%")
  (time (dolist (b b1)
	  (insert-object b r1)))
  (format t "Time for no overlapping inserts.~%")
  (time (dolist (b b2)
	  (insert-object b r2)))
  (format t "Time for a large number of inserts.~%")
  (time (dolist (b b3)
	  (insert-object b r3)))
  (format t "Time for single insert on large set with overlap.~%")
  (time (insert-object c0 r3))
  (format t "Time for single insert on large set without overlap.~%")
  (time (insert-object c1 r3))))

(defun test-overlap ()
  (let	((c0 (make-cube 0.0 0.0 0.0))
	 (c1 (make-cube 1.0 0.0 0.0))
	 (c2 (make-cube 1.5 0.0 0.0)))
    (format t "c1=~A~%c2=~A~%" (center (bounding-volume c1)) (center (bounding-volume c2)))
    (find-minimum-bound c0 (list c1 c2))))

(defun random-cube (max-pos max-size max-spin)
  (make-cube (random max-pos) (random max-pos) (random max-pos)
	     :color (vector (random 1.0) (random 1.0) (random 1.0) 0.2)
	     :scale-x (random max-size)
	     :scale-y (random max-size)
	     :scale-z (random max-size)
	     :rotate-x (random max-spin)
	     :rotate-y (random max-spin)
	     :rotate-z (random max-spin)))

(defun random-scene (n &key (pos 10.0) (size 2.0) (spin 180.0))
  (let ((r (make-instance 'r-tree :bounding-volume (make-instance 'bounding-box))))
    (dotimes (i n r)
      (insert-object (random-cube pos size spin) r))))

(defun r-tree-scene (n &optional (spacing 5.0))
  (let ((r (make-instance 'r-tree :bounding-volume (make-instance 'bounding-box))))
    (dolist (b (ordered-set-of-cubes n spacing) r)
      (insert-object b r))))

(defun test-intersection-sweep ()
  (let ((no-overlap (r-tree-scene 3))
	(overlap (r-tree-scene 3 0.5))
	(touching (r-tree-scene 3 1.0)))
    (format t "~A~%" (find-intersecting-set no-overlap))
    (format t "~A~%" (find-intersecting-set overlap))
    (format t "~A~%" (find-intersecting-set touching))
    overlap))
